#include "ti_msp_dl_config.h"
#include "motor.h"

// 静态变量存储当前PWM占空比
static uint16_t current_duty[4] = {0, 0, 0, 0};

// 静态变量存储系统启用状态
static bool system_enabled = false;

/**
 * @brief 电机驱动初始化
 */
void motor_init(void)
{
    // 初始化MOTOR_PWM_0 (TIMA1) 的PWM通道为0占空比
    DL_TimerA_setCaptureCompareValue(MOTOR_PWM_0_INST, 0, DL_TIMER_CC_0_INDEX);
    DL_TimerA_setCaptureCompareValue(MOTOR_PWM_0_INST, 0, DL_TIMER_CC_1_INDEX);
    
    // 初始化MOTOR_PWM_1 (TIMG7) 的PWM通道为0占空比
    DL_TimerG_setCaptureCompareValue(MOTOR_PWM_1_INST, 0, DL_TIMER_CC_0_INDEX);
    DL_TimerG_setCaptureCompareValue(MOTOR_PWM_1_INST, 0, DL_TIMER_CC_1_INDEX);
    
    // 启动PWM定时器
    DL_TimerA_startCounter(MOTOR_PWM_0_INST);
    DL_TimerG_startCounter(MOTOR_PWM_1_INST);
    
    // 清除所有GPIO控制引脚，确保电机处于停止状态
    // 前左电机控制引脚
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_FL_0_PORT, GPIO_TB6612_GPIO_FL_0_PIN);
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_FL_1_PORT, GPIO_TB6612_GPIO_FL_1_PIN);
    
    // 前右电机控制引脚
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_FR_0_PORT, GPIO_TB6612_GPIO_FR_0_PIN);
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_FR_1_PORT, GPIO_TB6612_GPIO_FR_1_PIN);
    
    // 后左电机控制引脚
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_BL_0_PORT, GPIO_TB6612_GPIO_BL_0_PIN);
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_BL_1_PORT, GPIO_TB6612_GPIO_BL_1_PIN);
    
    // 后右电机控制引脚
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_BR_0_PORT, GPIO_TB6612_GPIO_BR_0_PIN);
    DL_GPIO_clearPins(GPIO_TB6612_GPIO_BR_1_PORT, GPIO_TB6612_GPIO_BR_1_PIN);
    
    // 重置占空比记录
    for(int i = 0; i < 4; i++) {
        current_duty[i] = 0;
    }
    
    // 初始化系统状态（默认禁用）
    system_enabled = false;
}

/**
 * @brief 设置PWM占空比
 * @param channel PWM通道 (0-3): 0=PWM0_C0, 1=PWM0_C1, 2=PWM1_C0, 3=PWM1_C1
 * @param duty 占空比值 (0-3200)
 */
void motor_set_pwm_duty(uint8_t channel, uint16_t duty)
{
    // 限制占空比范围
    if(duty > MOTOR_PWM_MAX_DUTY) {
        duty = MOTOR_PWM_MAX_DUTY;
    }
    
    // 设置对应通道的PWM占空比
    switch(channel) {
        case 0: // MOTOR_PWM_0 C0
            DL_TimerA_setCaptureCompareValue(MOTOR_PWM_0_INST, duty, DL_TIMER_CC_0_INDEX);
            break;
        case 1: // MOTOR_PWM_0 C1
            DL_TimerA_setCaptureCompareValue(MOTOR_PWM_0_INST, duty, DL_TIMER_CC_1_INDEX);
            break;
        case 2: // MOTOR_PWM_1 C0
            DL_TimerG_setCaptureCompareValue(MOTOR_PWM_1_INST, duty, DL_TIMER_CC_0_INDEX);
            break;
        case 3: // MOTOR_PWM_1 C1
            DL_TimerG_setCaptureCompareValue(MOTOR_PWM_1_INST, duty, DL_TIMER_CC_1_INDEX);
            break;
        default:
            return;
    }
    
    current_duty[channel] = duty;
}

/**
 * @brief 获取PWM占空比
 * @param channel PWM通道 (0-3)
 * @return 当前占空比值
 */
uint16_t motor_get_pwm_duty(uint8_t channel)
{
    if(channel < 4) {
        return current_duty[channel];
    }
    return 0;
}

/**
 * @brief 设置H桥状态 - 四轮驱动版本
 * @param channel 电机通道 (左/右)
 * @param high_side 高侧开关状态
 * @param low_side 低侧开关状态
 */
void motor_set_bridge_state(motor_channel_t channel, bool high_side, bool low_side)
{
    if(channel == MOTOR_CHANNEL_LEFT) {
        // 左侧电机控制 (前左 + 后左)
        if(high_side) {
            DL_GPIO_setPins(GPIO_TB6612_GPIO_FL_1_PORT, GPIO_TB6612_GPIO_FL_1_PIN);  // 前左高侧
            // 后左电机方向反向：交换0和1引脚控制
            DL_GPIO_setPins(GPIO_TB6612_GPIO_BL_0_PORT, GPIO_TB6612_GPIO_BL_0_PIN);  // 后左反向控制
        } else {
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_FL_1_PORT, GPIO_TB6612_GPIO_FL_1_PIN);
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_BL_0_PORT, GPIO_TB6612_GPIO_BL_0_PIN);  // 后左反向控制
        }
        
        if(low_side) {
            DL_GPIO_setPins(GPIO_TB6612_GPIO_FL_0_PORT, GPIO_TB6612_GPIO_FL_0_PIN);  // 前左低侧
            // 后左电机方向反向：交换0和1引脚控制
            DL_GPIO_setPins(GPIO_TB6612_GPIO_BL_1_PORT, GPIO_TB6612_GPIO_BL_1_PIN);  // 后左反向控制
        } else {
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_FL_0_PORT, GPIO_TB6612_GPIO_FL_0_PIN);
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_BL_1_PORT, GPIO_TB6612_GPIO_BL_1_PIN);  // 后左反向控制
        }
    } else if(channel == MOTOR_CHANNEL_RIGHT) {
        // 右侧电机控制 (前右 + 后右)
        if(high_side) {
            DL_GPIO_setPins(GPIO_TB6612_GPIO_FR_1_PORT, GPIO_TB6612_GPIO_FR_1_PIN);  // 前右高侧
            // 后右电机方向反向：交换0和1引脚控制
            DL_GPIO_setPins(GPIO_TB6612_GPIO_BR_0_PORT, GPIO_TB6612_GPIO_BR_0_PIN);  // 后右反向控制
        } else {
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_FR_1_PORT, GPIO_TB6612_GPIO_FR_1_PIN);
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_BR_0_PORT, GPIO_TB6612_GPIO_BR_0_PIN);  // 后右反向控制
        }
        
        if(low_side) {
            DL_GPIO_setPins(GPIO_TB6612_GPIO_FR_0_PORT, GPIO_TB6612_GPIO_FR_0_PIN);  // 前右低侧
            // 后右电机方向反向：交换0和1引脚控制
            DL_GPIO_setPins(GPIO_TB6612_GPIO_BR_1_PORT, GPIO_TB6612_GPIO_BR_1_PIN);  // 后右反向控制
        } else {
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_FR_0_PORT, GPIO_TB6612_GPIO_FR_0_PIN);
            DL_GPIO_clearPins(GPIO_TB6612_GPIO_BR_1_PORT, GPIO_TB6612_GPIO_BR_1_PIN);  // 后右反向控制
        }
    }
}

/**
 * @brief 设置电机速度和方向
 * @param channel 电机通道
 * @param direction 运动方向
 * @param speed 速度值 (0-3200)
 */
void motor_set_speed(motor_channel_t channel, motor_direction_t direction, uint16_t speed)
{
    // 根据通道，确定要控制的PWM通道
    if(channel == MOTOR_CHANNEL_LEFT) {
        // 控制左侧两个电机 (PWM 0 和 PWM 1)
        motor_set_pwm_duty(0, speed); // 前左电机
        motor_set_pwm_duty(1, speed); // 后左电机
    } else {
        // 控制右侧两个电机 (PWM 2 和 PWM 3)
        motor_set_pwm_duty(2, speed); // 前右电机
        motor_set_pwm_duty(3, speed); // 后右电机
    }
    
    // 根据方向设置H桥状态
    switch(direction) {
        case MOTOR_DIRECTION_FORWARD:
            // 正转
            motor_set_bridge_state(channel, false, true);
            break;
            
        case MOTOR_DIRECTION_BACKWARD:
            // 反转
            motor_set_bridge_state(channel, true, false);
            break;
            
        case MOTOR_DIRECTION_BRAKE:
            // 制动
            motor_set_bridge_state(channel, true, true);
            // 制动时速度应为最大以确保锁死
            if(channel == MOTOR_CHANNEL_LEFT) {
                motor_set_pwm_duty(0, MOTOR_PWM_MAX_DUTY);
                motor_set_pwm_duty(1, MOTOR_PWM_MAX_DUTY);
            } else {
                motor_set_pwm_duty(2, MOTOR_PWM_MAX_DUTY);
                motor_set_pwm_duty(3, MOTOR_PWM_MAX_DUTY);
            }
            break;
            
        case MOTOR_DIRECTION_COAST:
            // 惯性滑行，将PWM占空比清零
            if(channel == MOTOR_CHANNEL_LEFT) {
                motor_set_pwm_duty(0, 0);
                motor_set_pwm_duty(1, 0);
            } else {
                motor_set_pwm_duty(2, 0);
                motor_set_pwm_duty(3, 0);
            }
            motor_set_bridge_state(channel, false, false);
            break;
    }
}

/**
 * @brief 设置左电机速度
 */
void motor_set_left_speed(motor_direction_t direction, uint16_t speed)
{
    motor_set_speed(MOTOR_CHANNEL_LEFT, direction, speed);
}

/**
 * @brief 设置右电机速度
 */
void motor_set_right_speed(motor_direction_t direction, uint16_t speed)
{
    motor_set_speed(MOTOR_CHANNEL_RIGHT, direction, speed);
}

/**
 * @brief 制动所有电机
 */
void motor_brake_all(void)
{
    motor_set_speed(MOTOR_CHANNEL_LEFT, MOTOR_DIRECTION_BRAKE, 0);
    motor_set_speed(MOTOR_CHANNEL_RIGHT, MOTOR_DIRECTION_BRAKE, 0);
}

/**
 * @brief 所有电机惯性滑行
 */
void motor_coast_all(void)
{
    motor_set_speed(MOTOR_CHANNEL_LEFT, MOTOR_DIRECTION_COAST, 0);
    motor_set_speed(MOTOR_CHANNEL_RIGHT, MOTOR_DIRECTION_COAST, 0);
}

/**
 * @brief 向前运动
 * @param speed 速度值 (0-3200)
 */
void motor_move_forward(uint16_t speed)
{
    motor_set_left_speed(MOTOR_DIRECTION_FORWARD, speed);
    motor_set_right_speed(MOTOR_DIRECTION_FORWARD, speed);
}

/**
 * @brief 向后运动
 * @param speed 速度值 (0-3200)
 */
void motor_move_backward(uint16_t speed)
{
    motor_set_left_speed(MOTOR_DIRECTION_BACKWARD, speed);
    motor_set_right_speed(MOTOR_DIRECTION_BACKWARD, speed);
}

/**
 * @brief 左转
 * @param speed 速度值 (0-3200)
 */
void motor_turn_left(uint16_t speed)
{
    motor_set_left_speed(MOTOR_DIRECTION_BACKWARD, speed);
    motor_set_right_speed(MOTOR_DIRECTION_FORWARD, speed);
}

/**
 * @brief 右转
 * @param speed 速度值 (0-3200)
 */
void motor_turn_right(uint16_t speed)
{
    motor_set_left_speed(MOTOR_DIRECTION_FORWARD, speed);
    motor_set_right_speed(MOTOR_DIRECTION_BACKWARD, speed);
}

/**
 * @brief 停止所有电机
 */
void motor_stop(void)
{
    motor_coast_all();
}

/**
 * @brief 启用电机系统
 * @note 设置TB6612的STBY引脚为高电平，允许电机运行
 */
void motor_enable_system(void)
{
    DL_GPIO_setPins(GPIO_MOTOR_PORT, GPIO_MOTOR_MORTOR_STBY_PIN);
    system_enabled = true;
}

/**
 * @brief 禁用电机系统
 * @note 设置TB6612的STBY引脚为低电平，强制停止所有电机
 */
void motor_disable_system(void)
{
    // 先停止所有PWM输出
    motor_coast_all();
    
    // 然后禁用系统
    DL_GPIO_clearPins(GPIO_MOTOR_PORT, GPIO_MOTOR_MORTOR_STBY_PIN);
    system_enabled = false;
}

/**
 * @brief 检查电机系统是否启用
 * @return true: 系统已启用, false: 系统已禁用
 */
bool motor_is_system_enabled(void)
{
    return system_enabled;
}   

